\section{Controller Design}
This section answers questions related to the controller design of the robot.

\subsection{Question 7}
Figure \ref{fig:mechsd} shows the mechanical spring-damper system that is to be analyzed. 
\begin{figure}
  \begin{center}
    \includegraphics{mechsd.eps}
  \end{center}
  \caption{Mechanical Spring Damper System}
  \label{fig:mechsd}
\end{figure}
The torque exerted on this system is shown below. 
\begin{equation}
  T = -k(q-q_{ref})-b\frac{dq}{dt}
\end{equation}
Comparing against the equation given in the handout:
\begin{equation}
  \begin{split}
  T = u \\
  k_p = k \\
  k_v = b \\
  \end{split}
\end{equation}
For something to be unstable, the reaction must get bigger and amplified when trying to reach the desired position. This would require overshoots that would get larger everytime. It would also required the opposite effect of a damper that encourages the oscillation for every cycle rather than discourage it. With the damper, the system will always be stable. 

\subsection{Question 8}
The general controller setup for each motor is shown in Figure \ref{fig:consetup}.
\begin{figure}
  \begin{center}
    \includegraphics[width=12.0cm]{controller_setup.eps}
  \end{center}
  \caption{Inverse Dynamics Controller}
  \label{fig:consetup}
\end{figure}
Knowing the new dynamics equations after considering that the gravitational effect on motor 2 has been eliminated and the gravitational effct on motor 1 has been reduced:
\begin{equation}
  T_1 = ( (M)_{11} + I^1_h ) \ddot q_1 + g(m_1 l_{c1} + m_3 l_{c3} + m_4 l_1)C_{q1} - k (\frac{\pi}{2} - q_1)
\end{equation}
\begin{equation}
  T_2 = ( (M)_{22} + I^2_h ) \ddot q_2
\end{equation}
Taken into account these torques for the general equation:
\begin{equation}
  J_i \ddot q_i + B_i \dot q_i = u - T_i
\end{equation}
The new controller equations can be observed for motor 1 and motor 2 respectively.
\begin{equation}
  u_1 = ( (M)_{11} + I^1_h + J_1) \ddot q_1 + B_1\dot q_1 + g(m_1 l_{c1} + m_3 l_{c3} + m_4 l_1)C_{q1} - k (\frac{\pi}{2} - q_1)
\end{equation}
\begin{equation}
  u_2 = ( (M)_{22} + I^2_h +J_2) \ddot q_2 + B_2\dot q_2
\end{equation}
To make this system of equations linear, the $\ddot q_i$ values can be replaced with $v_i$ instead. 
\begin{equation}
  \label{eq:u_1}
  u_1 = ( (M)_{11} + I^1_h + J_1) v_1 + B_1\dot q_1 + g(m_1 l_{c1} + m_3 l_{c3} + m_4 l_1)C_{q1} - k (\frac{\pi}{2} - q_1)
\end{equation}
\begin{equation}
  \label{eq:u_2}
  u_2 = ( (M)_{22} + I^2_h +J_2) v_2 + B_2\dot q_2
\end{equation}
This follows the general equation for the inverse-dynamics controller.
\begin{equation}
  u=m\ddot q+h(q,\dot q)
\end{equation}
Equations (\ref{eq:u_1}) and (\ref{eq:u_2}) can be used as the non-linear interface for the inner-loop control. Thus the only inputs it would require would come from the Linear compensator that gives $v_1$ and $v_2$. These compensators can be set as follows.
\begin{equation}
  \label{eq:v_1}
  v_1 = -k_p(q_1-q^d_{1})-k_v\dot q_1
\end{equation}
\begin{equation}
  \label{eq:v_2}
  v_2 = -k_p(q_2-q^d_{2})-k_v\dot q_2
\end{equation}
Equations (\ref{eq:v_1}) and (\ref{eq:v_2}) can be used as the linear compensators for the outer-loop control. 
